#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <geometry_msgs/Pose.h>

int main(int argc, char** argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "moveit_cpp_example");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(1); // 使用异步 spinner 处理回调
    spinner.start();

    // 初始化 MoveIt 接口
    static const std::string PLANNING_GROUP = "manipulator"; // 修改为你的规划组名称
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    move_group.setMaxAccelerationScalingFactor(0.8);
    move_group.setMaxVelocityScalingFactor(0.8);

    // 输出一些信息
    ROS_INFO("Reference frame: %s", move_group.getPlanningFrame().c_str());
    ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());

    // 设置随机目标点
    move_group.setRandomTarget();

    // 规划路径
    ROS_INFO("Planning to target position...");
    moveit::planning_interface::MoveGroupInterface::Plan plan;

    // 运动学逆解：规划路径到目标点
    if (move_group.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS)
    {
        ROS_INFO("Planning successful!");
        ROS_INFO("Planning Time: %f seconds", plan.planning_time_);

        // 打印路径的所有信息
        const auto& trajectory = plan.trajectory_.joint_trajectory;

        // 打印关节名称
        ROS_INFO("Joint Names:");
        for (const auto& joint_name : trajectory.joint_names)
        {
            ROS_INFO("  %s", joint_name.c_str());
        }

        // 打印路径包含的关键点数量
        ROS_INFO("Trajectory contains %ld waypoints.", trajectory.points.size());

        // 遍历每个路径点，打印详细信息
        for (size_t i = 0; i < trajectory.points.size(); ++i)
        {
            const auto& point = trajectory.points[i];
            ROS_INFO("Waypoint %ld:", i + 1);

            // 打印每个关节的位置、速度、加速度信息
            for (size_t j = 0; j < point.positions.size(); ++j)
            {
                ROS_INFO("  Joint %s:", trajectory.joint_names[j].c_str());
                ROS_INFO("    Position     = %.6f", point.positions[j]);
                ROS_INFO("    Velocity     = %.6f", point.velocities[j]);;
                ROS_INFO("    Acceleration = %.6f", point.accelerations[j]);
            }

            // 打印时间信息
            ROS_INFO("  Time from start = %.6f seconds", point.time_from_start.toSec());
        }

        // 执行运动
        ROS_INFO("Executing the planned motion...");
        move_group.move();
    }
    else
    {
        ROS_WARN("Planning failed. Skipping movement to target position.");
    }

    // 运动学正解：返回 Home 点
    std::vector<double> home_joint_positions = {0.0, -1.57, 1.57, 0.0, 1.57, 0.0}; // 修改为你的机械臂的 Home 点关节值
    move_group.setJointValueTarget(home_joint_positions);

    ROS_INFO("Planning to Home position...");
    if (move_group.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS)
    {
        ROS_INFO("Planning successful to Home position!");
        move_group.move();
    }
    else
    {
        ROS_WARN("Planning failed. Skipping movement to Home position.");
    }

    // 关闭 ROS 节点
    ros::shutdown();
    return 0;
}
